#ifndef _PID_H
#define _PID_H
#include "sys.h"

#define min(a, b) ((a) < (b) ? (a) : (b))
#define max(a, b) ((a) > (b) ? (a) : (b))
#define ABS(x) ((x) > 0 ? (x) : -(x))
#define LIMIT( x,min,max ) ( (x) < (min)  ? (min) : ( (x) > (max) ? (max) : (x) ) )

struct PositionPID{
	float kp, ki, kd;
	float error, last_error, sum_error;
};


struct IncrementalPID{
	float kp, ki, kd;
	float error, last_error, last_last_error;
};


void position_pid_init(struct PositionPID* p, float kp, float ki, float kd);
void incremental_pid_init(struct IncrementalPID* p, float kp, float ki, float kd);
float position_pid(struct PositionPID* p);
float incremental_pid(struct IncrementalPID* p);

#endif
